/******************************************************************************
COPYRIGHT (C) 2013 by IT-Designers GmbH

File:		RoadLane.cpp
Created:	07.03.2013
Modified:	11.04.2013
Author:		Max Pejs
******************************************************************************/

#include "RoadLane.h"

bool findNextPoint(Mat & src, Point2i & srcPoint, Point2i & dstPoint, int lowlvl = 100, int highlvl = 200);

RoadLane::RoadLane(){}

RoadLane::RoadLane(LaneType lp):LaneTp(lp)
{
	BasePoints = vector<Point2d>(5);
}

RoadLane::~RoadLane()
{
	BasePoints = vector<Point2d>(5);
}

Line2D RoadLane::GetSingleLinePart(unsigned int ind)
{
	return LinePartsVector[ind];
}

unsigned int RoadLane::LinePartsCnt()
{
	return LinePartsVector.size();
}

void RoadLane::PlotParameters(Mat & img, CvScalar & color, Point & offset)
{
	for(unsigned int i = 0; i < LinePartsVector.size(); i++)
	{
		LinePartsVector[i].PlotParameters(img, color, offset);
	}
}

Line2D RoadLane::GetCenterLine()
{
	return centerLine;
}

void RoadLane::Find(Mat & binaryImg)
{
	/****************************************************/
	/************ Liniendetektion mit HoughLinesP *******/
	/****************************************************/

	vector<Vec4i> lines;
	HoughLinesP(binaryImg, lines, 1, CV_PI/180, 60, MIN_LINE_LENGTH, 50);

	/****************************************************/
	/************ Analyse der gefundenen Linien	*********/
	/****************************************************/

	lines2Cluster(lines);
}

void RoadLane::lines2Cluster(vector<Vec4i> & lines)
{
	Mat img;
	int maxDiff = 100;
	vector<Line2D> result;

	// filter lines with grater rho tnah allowed
	for( size_t i = 0; i < lines.size(); i++ )
	{
		Line2D r(lines[i]);

		if( abs(r.Rho) < ALLOWED_LINE_SKEW )
		{
			result.push_back(r);
		}
	}

	// If no lines are left
	// TODO
	if(result.size() == 0)
	{
		LinePartsVector.clear();
		return;
	}

	Mat points(result.size(), 2, CV_32FC1), labels, centers;

	double compactness;

	for( size_t i = 0; i < result.size(); i++ )
	{
		points.at<float>(i,0) = (float)result[i].Rho;
		points.at<float>(i,1) = (float)result[i].Theta;
	}

	TermCriteria tc( CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 10, 1.0);

	compactness = kmeans(points, 1, labels, tc, 3, KMEANS_PP_CENTERS ,centers);

	float rhoCenter		= centers.at<float>(0, 0);
	float thetaCenter	= centers.at<float>(0, 1);
	Point2f centerPoint(rhoCenter, thetaCenter);

	vector<Line2D> v;

	for( unsigned int i = 0; i < result.size(); i++ )
	{
		v.push_back(result[i]);
	}

	int weight = 5;

	double x = centerPoint.x;
	double y = centerPoint.y;

	LinePartsVector = v;

	double lowerLim = 0.0;
	double upperLim = 480.0;	// ::TO DEFINE

	if(x == 0.0)
		x = centerLine.Rho;

	if(y == 0.0)
		y = centerLine.Theta;

	x = ((centerLine.Rho * 4) + x) / 5;
	y = ((centerLine.Theta * 4) + y) / 5;

	centerLine = Line2D(x, y, upperLim, lowerLim);
}

void RoadLane::Match(Mat & src, Mat & templ)
{
	Mat result;

	int step = src.rows / (BasePoints.size()-1);
	int start = templ.rows / 2;

	Point minLoc;
	double maxValue, minValue;
	Mat sliceImg;

	for(unsigned int k = 0; k < BasePoints.size(); k++)
	{
		Rect slice(0, start, src.cols, templ.rows);
		sliceImg = src(slice);

		matchTemplate(sliceImg, templ, result, CV_TM_SQDIFF );
		minMaxLoc(result, &minValue, &maxValue, &minLoc);

		minLoc.y = start + templ.rows;
		minLoc.x += templ.cols / 2;

		double delta = maxValue - minValue;

		if(delta > 3500000)
		{
			BasePoints[k] = minLoc;
		}

		/*BasePoints[k].x = ((BasePoints[k].x * 3) + minLoc.x) / 4;
		BasePoints[k].y = ((BasePoints[k].y * 3) + minLoc.y) / 4;*/

		start += step;

		if(k == BasePoints.size() - 2)
		{
			start = (src.rows - (int)(src.rows * 0.1));
		}
	}
}

vector<Point2d> & RoadLane::GetBasePoints()
{
	return BasePoints;
}

int RoadLane::DetermineLaneType(Mat & img)
{
	Mat imgCopy = img.clone();
	vector<vector<Point>> contours0;
	/* vector<vector<Point>>contours;
	contours.resize(contours0.size()); */

	findContours(img, contours0, CV_RETR_EXTERNAL, CHAIN_APPROX_SIMPLE);
	// drawContours(img, contours0, -1, Scalar(127,127,127), 1, CV_AA);

	double area = 0.0;
	int cnt = contours0.size();
	for(auto el = contours0.begin(); el != contours0.end(); el++)
	{
		area += contourArea(*el);
	}

	return -1;
}

void RoadLane::Move(Point2d & offset)
{
	// center line
	centerLine.Move(offset);

	// all lines
	for(auto el = LinePartsVector.begin(); el != LinePartsVector.end(); el++)
		(*el).Move(offset);
}

void RoadLane::Transform(Mat & H)
{
	// center point
	centerLine.Transform(H);

	// all lines
	for(auto el = LinePartsVector.begin(); el != LinePartsVector.end(); el++)
		(*el).Transform(H);
}

void RoadLane::PlotBasePoints(Mat & img, CvScalar & color)
{
	for(auto el = BasePoints.begin(); el != BasePoints.end(); ++el)
		circle(img, Point2d((*el).x, (*el).y), 2, color, 2);
}

void RoadLane::PlotLineParts(Mat & img, CvScalar & color,	bool plotCenterLine /* = false */)
{
	for(auto el = LinePartsVector.begin(); el != LinePartsVector.end(); ++el)
		(*el).plotLineToImg(img, color);

	if(plotCenterLine)
	{
		CHANGE_COLOR(color);
		this->GetCenterLine().plotLineToImg(img, color);
	}
}